﻿#ifndef CLSCALCULATEAB_H
#define CLSCALCULATEAB_H

#include <QObject>
#include <QSettings>
#include <qvariant.h>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <halconcpp/HalconCpp.h>

#ifdef max
#undef max
#endif
#ifdef min
#undef min
#endif

#include "FLANN/include/flann/defines.h"
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/normal_space.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/filter.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/filters/crop_hull.h>
#include <pcl/surface/convex_hull.h>

class ClsCalculateAB
{
public:
	ClsCalculateAB();
	/// <summary>
	/// 计算变换矩阵B
	/// </summary>
	/// <param name="cloud">世界系深度图</param>
	/// <param name="B">矩阵B</param>
	/// <param name="minx"></param>
	/// <param name="miny"></param>
	/// <param name="dx"></param>
	/// <param name="dy"></param>
	/// <returns></returns>
	bool GetMatrixB_Point(const cv::Mat& cloud, cv::Mat& B, double& minx, double& miny, float dx = 0.1, float dy = 0.1);
	bool GetMatrixB_Point(const cv::Mat& cloud, double& minx, double& miny, float dx = 0.1, float dy = 0.1);
	/// <summary>
	/// 位姿转位姿矩阵
	/// </summary>
	/// <param name="pos">位姿</param>
	/// <param name="matrix">位姿矩阵</param>
	void PosToMat(float x, float y, float z, float a, float b, float c, cv::Mat& matrix);
	/// <summary>
	/// 位姿矩阵转位姿
	/// </summary>
	/// <param name="matrix">位姿矩阵</param>
	/// <param name="pos">位姿</param>
	void MatToPos(const cv::Mat& matrix, QStringList& pos, const QStringList& originPos = QStringList());
	void MatToPosDiff(const cv::Mat& matrix, QStringList& diffList, const QStringList& originPos = QStringList());
	/// <summary>
	/// 位姿变换
	/// </summary>
	/// <param name="pos">位姿矩阵</param>
	/// <param name="trans">转换矩阵A/B</param>
	/// <param name="newpos">新位姿矩阵</param>
	bool PosTrans(const cv::Mat& pos, const cv::Mat& trans, cv::Mat& newpos);
	/// <summary>
	/// 点云变换
	/// </summary>
	/// <param name="cloud">点云</param>
	/// <param name="trans">相机标定矩阵</param>
	/// <param name="pos">位姿矩阵</param>
	/// <param name="newcloud">点云</param>
	/// <param name="id">矫正mark</param>
	bool CloudTrans(const cv::Mat& cloud, const cv::Mat& trans, const cv::Mat& pos, cv::Mat& newcloud, float deltaX, float deltaY, float deltaZ);
	bool CloudTrans(const cv::Mat& cloud, const cv::Mat& trans, const cv::Mat& pos, cv::Mat& newcloud);
	bool CloudTrans(const cv::Mat& cloud, const cv::Mat& trans, cv::Mat& newcloud);
	/// <summary>
	/// 深度图转换
	/// </summary>
	/// <param name="transedXyz">世界系xyz</param>
	/// <param name="newDepth">深度图</param>
	/// <param name="minx"></param>
	/// <param name="miny"></param>
	/// <param name="dx"></param>
	/// <param name="dy"></param>
	/// <returns></returns>
	bool DepthTrans(const cv::Mat& transedXyz, cv::Mat& newDepth, double& minx, double& miny, float dx = 0.1, float dy = 0.1);
	/// <summary>
	/// 点云采样
	/// </summary>
	/// <param name="cloud">原始点云xyz</param>
	/// <param name="samplingGap">采样间隔</param>
	/// <param name="rows">图像行数</param>
	/// <param name="cols">图像列数</param>
	/// <param name="sampledCloud">采样后xyz</param>
	/// <returns></returns>
	bool Sampling(const cv::Mat& cloud, int samplingGap, int rows, int cols, cv::Mat& sampledCloud);
	/// <summary>
	/// 初始化
	/// </summary>
	bool Init();
	/// <summary>
	/// 设置参数
	/// </summary>
	/// <param name="param"></param>
	void SetParams(const QMap<QString, QString>& param);
	/// <summary>
	/// 清除新mark坐标，精定位结束调用
	/// </summary>
	void ClearNewMarkTuple();
	/// <summary>
	/// 获取错误信息
	/// </summary>
	/// <returns></returns>
	QString GetLastErrorMsg()const;
	/// <summary>
	/// 法向采样
	/// </summary>
	/// <param name="cloud">输入点云</param>
	/// <param name="k">计算法向时候的近邻点</param>
	/// <param name="sample_ratio">采样比例，剩余点数 = 原始点云数*sample_ratio</param>
	/// <param name="kBinNum">法向分类数量</param>
	/// <returns>输出点云</returns>
	cv::Mat normalspacesample(cv::Mat cloud_Mat, int width, int height, int k, float sample_ratio, int kBinNum);

	pcl::PointCloud<pcl::PointXYZ>::Ptr CvMatToPclPointXYZ(const cv::Mat& cloud, int width, int height);
	cv::Mat PclPointXYZToCvMat(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
	/// <summary>
	/// 开班检测
	/// </summary>
	/// <param name="xyz"></param>
	/// <param name="gray"></param>
	/// <param name="handEye"></param>
	/// <param name="posMat"></param>
	/// <returns></returns>
	bool FirstCheck(const cv::Mat& xyz,const cv::Mat& gray, const cv::Mat& handEye, const cv::Mat& posMat);

	void TestAlg(const cv::Mat& in, cv::Mat& out);

	void EuclideanConnection(const cv::Mat& cloud, cv::Mat& out);
	void OutlineRemoval(const cv::Mat& cloud, cv::Mat& out,int x,int y);
	bool Smooth(const cv::Mat& cloud, cv::Mat& out);

	//void AddCalib(const cv::Mat& cloud, cv::Mat& out, int x, int y, int z);
	void AddCalib(const cv::Mat& cloud, cv::Mat& out, float x, float y, float z);
	void AddCalibOffset(const cv::Mat& cloud, cv::Mat& out, float x, float y, float z);

	bool ReduceClouPoint(cv::Mat& MatInData, std::vector<float> vfPolypointxy);
	bool ReduceClouPoint(cv::Mat& MatInData, std::string boundingPCLPath);
	bool ReduceByPoint3d(const pcl::PointCloud<pcl::PointXYZ>::Ptr& source,\
							pcl::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> reducePoint, \
							pcl::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> result);

private:
	struct CalibratePos
	{
		CalibratePos()
			:_deltaX(0)
			, _deltaY(0)
			, _deltaZ(0)
		{}
		float _deltaX;
		float _deltaY;
		float _deltaZ;
	};

	QString ReadValue(const QString& strSection, const QString& Key, const QString& str_INIPath);

	bool FindCornerPoints(const cv::Mat& gray, cv::Mat& points);

	bool GetPlane(const cv::Mat& cloud, float* A, float* B, float* C, float* D);

	bool GetEdgeCornerPoint(const std::vector<CalibratePos>& corner, std::vector<CalibratePos>& edgeCorner);

	cv::Mat TransformWithOrigin(const cv::Mat& cloud, const cv::Mat& mat);

	QMap<QString, QString> mAlgParam;

	QVector<float> v_mark;

	int mRadius{ 190 };//GetMatrixB_Cloud
	float mSampleB{ 0.03 };//GetMatrixB_Cloud 
	int mAreaSmall{ 5165 };//GetMatrixB_Cloud 
	int mAreaBig{ 70454 };//GetMatrixB_Cloud

	QString mErrorMsg{ "No Error" };

	HalconCpp::HTuple mNewMarkX;
	HalconCpp::HTuple mNewMarkY;
	HalconCpp::HTuple mNewMarkZ;
};

bool create_PCL_from_txt(const std::string& file_path, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
#endif // CLSCALCULATEAB_H
